#include "devices.h"
#include "at91sam7s.h"
#include "jtag.h"

void test0(void)
{
    unsigned n;

#if 1
    jtag_reset();
    jtag_halt_cpu(0);
    jtag_select_debug();

//    dprintf("00000000: %x\n", jtag_reg_rd(0));
    dprintf("00000000: %x\n", jtag_mem_rd(0));
    return ;
#endif
    
    jtag_reset();
    n = jtag_get_id();
    dprintf("JTAG IDCODE = %x\n", n);

    if(n != 0x3f0f0f0f) {
        dprintf("ARM7TDMI not detected\n");
        return;
    }
    
    jtag_select_ice();
    dprintf("%x\n", jtag_ice_rd(ICE_DEBUG_STATUS));

    jtag_halt_cpu(1);
    jtag_select_debug();
    dprintf("PC: %x\n", jtag_reg_rd(15));
    
#if 0    
    for(n = 0; n < 16; n++){
        dprintf("R%d = %x\n", n, jtag_reg_rd(n));
    }
#endif

    dprintf("00000000: %x\n", jtag_mem_rd(0));    
    dprintf("00000004: %x\n", jtag_mem_rd(4));

//    dprintf("CPSR: %x\n", jtag_cpsr_rd());

    dprintf("00202000: %x\n", n = jtag_mem_rd(0x202000));
    jtag_mem_wr(0x202000, n + 1);
    dprintf("00202000: %x\n", n = jtag_mem_rd(0x202000));
    
#if 0
    for(n = 0; n < 16; n++){
        dprintf("R%d = %x\n", n, jtag_reg_rd(n));
    }

    jtag_select_ice();
    dprintf("%x\n", jtag_ice_rd(ICE_DEBUG_STATUS));
#endif
}

void init_48mhz_clock();

void boot(void)
{
#if 0
    AT91PIO *pio = AT91PIO_ADDR;
    AT91PMC *pmc = AT91PMC_ADDR;
    unsigned n, on;

//    init_48mhz_clock();
    
        /* make sure these are high before we make them outputs,
           so we don't get any glitches during startup */
    pio->data_set = (PIN_TDI | PIN_TMS | PIN_TCK | PIN_nSRST);

        /* we want to be able to update all 3 of the JTAG
           output bits with one write */
    pio->write_disable = 0xffffffff;
    pio->write_enable = PIN_TDI | PIN_TMS | PIN_TCK;
    
    pio->output_disable = BOARD_OUTPUT_DISABLE;
    pio->output_enable = BOARD_OUTPUT_ENABLE;
    pio->pullup_disable = BOARD_PULLUP_DISABLE;
    pio->pullup_enable = BOARD_PULLUP_ENABLE;
    pio->select_a = BOARD_SELECT_A;
    pio->select_b = BOARD_SELECT_B;
    pio->pio_disable = BOARD_PIO_DISABLE;
    pio->pio_enable = BOARD_PIO_ENABLE;

    
    ser_init();

    test0();

    dprintf("@done@\n");
    
    on = 0;
    n = 0;
	for(;;) {
        if(n++ < 1000000) continue;
        n = 0;
        if(on) {
            pio->data_set = PIN_LED_YELLOW;
            on = 0;
        } else {
            pio->data_clear = PIN_LED_YELLOW;
            on = 1;
        }
    }
#else
    jtag_init();
    ser_init();
    test0();
    dprintf("@done@\n");
    for(;;);
#endif
    
}

